package fr.uha.ensisa.jmp.drone.tests.protocol;

import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.InetSocketAddress;
import java.net.NetworkInterface;
import java.net.SocketException;
import java.net.UnknownHostException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Enumeration;

import fr.uha.ensisa.jmp.drone.app.DroneConstants;


public class Test
{
	
	public static void testInterface() throws SocketException
	{
			Enumeration interfaces = NetworkInterface.getNetworkInterfaces( );

			while (interfaces.hasMoreElements( )) 
			{
				NetworkInterface ni = (NetworkInterface) interfaces.nextElement( );
				System.out.println(ni);               
	      }  	
			
			NetworkInterface en1 = NetworkInterface.getByName("en1");

			Enumeration addresses = en1.getInetAddresses( );

			while (addresses.hasMoreElements( )) {
			  System.out.println(addresses.nextElement( ));    
			}
	}
		
	public static void session2() throws IOException, InterruptedException
	{
		final String WIFI_NIF = "en1";
		final String WIFI_ARDRONE_IP = "192.168.1.1";
		final int NAVDATA_PORT= 5554;
		final int ATCOMMAND_PORT= 5556;

		final int ARDRONE_NAVDATA_BOOTSTRAP		= 1 << 11; /*!< Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */	
		//final int ARDRONE_COMMAND_MASK			= 1 << 6;  /*!< Control command ACK : (0) None, (1) one received */
		//final int ARDRONE_CTRL_WATCHDOG_MASK		= 1 << 28; /*!< CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */
		//final int ARDRONE_COM_WATCHDOG_MASK		= 1 << 30; /*!< Communication Watchdog : (1) com problem, (0) Com is ok */
		
		NetworkInterface nif = NetworkInterface.getByName(WIFI_NIF);
		System.out.println(nif);
		Enumeration<InetAddress> nifAddresses = nif.getInetAddresses();
		InetAddress localAddress = nifAddresses.nextElement();
		
		DatagramSocket nav_socket = new DatagramSocket(NAVDATA_PORT, localAddress);
		DatagramSocket at_socket = new DatagramSocket( ATCOMMAND_PORT, localAddress);
		
		nav_socket.setSoTimeout(3000);
		at_socket.setSoTimeout(3000);

		InetAddress inet_addr = InetAddress.getByName(WIFI_ARDRONE_IP);

		ByteBuffer bb = ByteBuffer.allocate(2048);
		bb.putInt(1);		

	
		Thread.sleep(1);
		System.out.println("AT*COMWDG=1\r");
		byte[] cdeBuffer = "AT*COMWDG=1\r".getBytes();
		DatagramPacket at_packet= new DatagramPacket(cdeBuffer, cdeBuffer.length, inet_addr, ATCOMMAND_PORT);
		at_socket.send(at_packet);

		byte[] buffer = {0x01, 0x00, 0x00, 0x00};
		DatagramPacket nav_packet = new DatagramPacket(buffer, buffer.length, inet_addr, NAVDATA_PORT);
		nav_socket.send(nav_packet);
		nav_socket.receive(nav_packet); 
		
		System.out.println(nav_packet.getLength());

		bb = ByteBuffer.wrap(nav_packet.getData()).order(ByteOrder.LITTLE_ENDIAN);
		int header = bb.getInt();
		int state = bb.getInt();
		int seq = bb.getInt();
		int vflag = bb.getInt();
		short oid = bb.getShort();
		
		System.out.println("header : " + Integer.toHexString(header));
		System.out.println("state : " + Integer.toBinaryString(state));
		System.out.println("seq : " + seq);
		System.out.println("video flag : " + Integer.toBinaryString(vflag));
		System.out.println("oid : "+ Integer.toBinaryString(oid));


		
		System.out.println(Integer.toBinaryString(ARDRONE_NAVDATA_BOOTSTRAP));
		
		
		if((state & ARDRONE_NAVDATA_BOOTSTRAP) == ARDRONE_NAVDATA_BOOTSTRAP)
		{
			System.out.println("nav data in bootstrap mode...");
			
			Thread.sleep(1);
			System.out.println("AT*CONFIG=\"general:navdata_demo\",\"TRUE\"\r");
			cdeBuffer = "AT*CONFIG=\"general:navdata_demo\",\"TRUE\"\r".getBytes();
			at_packet= new DatagramPacket(cdeBuffer, cdeBuffer.length, inet_addr, ATCOMMAND_PORT);
			at_socket.send(at_packet);
	
			bb.clear();
			nav_packet = new DatagramPacket(buffer, buffer.length, inet_addr, NAVDATA_PORT);
			nav_socket.receive(nav_packet); 

			System.out.println(nav_packet.getLength());

			bb = ByteBuffer.wrap(nav_packet.getData()).order(ByteOrder.LITTLE_ENDIAN);
			header = bb.getInt();
			state = bb.getInt();
			seq = bb.getInt();
			vflag = bb.getInt();
			oid = bb.getShort();
			
			System.out.println("header : " + Integer.toHexString(header));
			System.out.println("state : " + Integer.toBinaryString(state));
			System.out.println("seq : " + seq);
			System.out.println("video flag : " + Integer.toBinaryString(vflag));
			System.out.println("oid : "+ Integer.toBinaryString(oid));

			//-----------------------------------------------------------------------
			Thread.sleep(1);
			System.out.println("AT*CTRL=3\r");
			cdeBuffer = "AT*CTRL=3\r".getBytes();
			at_packet= new DatagramPacket(cdeBuffer, cdeBuffer.length, inet_addr, ATCOMMAND_PORT);
			at_socket.send(at_packet);


			long t = System.currentTimeMillis();
			while(true)
			{
				Thread.sleep(1);
				bb.clear();
				nav_packet = new DatagramPacket(buffer, buffer.length, inet_addr, NAVDATA_PORT);
				nav_socket.receive(nav_packet); 
	
				System.out.println(nav_packet.getLength());
	
				bb = ByteBuffer.wrap(nav_packet.getData()).order(ByteOrder.LITTLE_ENDIAN);
				header = bb.getInt();
				state = bb.getInt();
				seq = bb.getInt();
				vflag = bb.getInt();
				oid = bb.getShort();
	
				System.out.println("header : " + Integer.toHexString(header));
				System.out.println("state : " + Integer.toBinaryString(state));
				System.out.println("seq : " + seq);
				System.out.println("video flag : " + Integer.toBinaryString(vflag));
				System.out.println("oid : "+ Integer.toBinaryString(oid));
				
				System.out.println("time : "+ (System.currentTimeMillis()-t));
			}

		}
		
		/*
		

		cdeBuffer = "AT*CTRL=0".getBytes();
		at_packet= new DatagramPacket(cdeBuffer, cdeBuffer.length, inet_addr, ATCOMMAND_PORT);
		at_socket.send(at_packet);
		
		nav_socket.receive(nav_packet); 
		//bb = ByteBuffer.wrap(nav_packet.getData());
		//response = bb.getInt();
		System.out.println(nav_packet.getLength());
		nav_socket.receive(nav_packet); 
		System.out.println(nav_packet.getLength());

		//System.out.println(packet.getLength());
		//System.out.println(new String(packet.getData(),0,packet.getLength()));
		  //soc.connect(new InetSocketAddress(address, port));
		  
		*/
	}
	
	public static void testUDP()
	{
		final int TEST_ITERATION = 100;
		final int TEST_PORT = 8532;
		
		Runnable server = new Runnable()
		{
			public void run()
			{
				try
				{
					InetAddress clientAddress = InetAddress.getLocalHost();
					System.out.println(clientAddress);

					DatagramSocket socket = new DatagramSocket();
					socket.setSoTimeout(3000);
					
					ByteBuffer bb = ByteBuffer.allocate(4);

					
					for(int i=1; i< TEST_ITERATION; i++)
					{
						bb.putInt(i);
						byte[] buffer =bb.array();
						DatagramPacket packet= new DatagramPacket(buffer,buffer.length, clientAddress, TEST_PORT);
						socket.send(packet);
						bb.clear();
						System.out.println("server : " + i);
						Thread.sleep(10);
					}
					
				}
				catch (SocketException e)
				{
					e.printStackTrace();
				}
				catch (UnknownHostException e)
				{
					e.printStackTrace();
				}
				catch (IOException e)
				{
					e.printStackTrace();
				}
				catch (InterruptedException e)
				{
					e.printStackTrace();
				}
				
			}
			
		};
		
			
		
		Runnable client = new Runnable()
		{
			public void run()
			{
				try
				{
					InetAddress serverAddress = InetAddress.getLocalHost();
					System.out.println(serverAddress);
					DatagramSocket socket = new DatagramSocket(TEST_PORT);
					socket.setSoTimeout(3000);

					
					byte[] receiveBuffer = new byte[4];
					DatagramPacket packet = new DatagramPacket(receiveBuffer, receiveBuffer.length, serverAddress, TEST_PORT);
					ByteBuffer bb = ByteBuffer.wrap(receiveBuffer);
					
					for(int i=1; i< TEST_ITERATION; i++)
					{
						bb.clear();
						socket.receive(packet);
						System.out.println(bb.getInt());
					}
					
				}
				catch (SocketException e)
				{
					e.printStackTrace();
				}
				catch (UnknownHostException e)
				{
					e.printStackTrace();
				}
				catch (IOException e)
				{
					e.printStackTrace();
				}
				
			}
		};

		new Thread(client).start();
		new Thread(server).start();
		
		
	}
	public static void main(String[] args)
	{
		try
		{
			//session1();
			session2();
			//testInterface();
			//testUDP();
		}
		catch (Exception e)
		{
			e.printStackTrace();
		}
	}
}
